package edu.tec.mty.players;

import edu.tec.mty.core.*;
import edu.tec.mty.core.modules.Game;
import edu.tec.mty.core.modules.Self;
import edu.tec.mty.core.modules.Server;
import edu.tec.mty.core.modules.models.*;
import edu.tec.mty.extras.*;
import edu.tec.mty.extras.fuzzy.*;

abstract public class PlayerActions {

	public Commands commands;
	public Game game;
	public Self player;
	
	
	public int addDelay = 3;							//Additional delay required from some specific functions
	public int delayWait = 2;							//Delay of receiving and executing messages
	public double positionThreshold = 0.75;				//Threshold for position
	public double directionThreshold = 1.0;			//Threshold for direction
	public double chgRulesThreshold = 20.0;				//Threshold for rules change
	
	
	public String playMode;								//Current PlayMode
	public double neckAng, neckSearchMoment;			//Angle of Neck, Moment for neck turn in searchBall
	public double exploreMoment;						//Moment for explore
	public double turnMoment;							//Moment for turns
	public boolean turnOrderSent = false;				//Turn Order Flag
	public boolean neckOrderSent = false;				//Neck Order Flag
	public boolean exploreOrderSent = false;			//Explore Neck Order Flag
	public boolean changeViewSent = false;				//Change View Order Flag
	public boolean dashSent = false;					//Dash Order Flag
	public boolean turnSent = false;					//Turn Order Flag
	public boolean noDash = false;						//Allows or Deny Dash
	public long lastNeckOrderTime = 0;					//Cycle when last neck order was sent
	public long lastExploreOrderTime = 0;				//Cycle when last explore order was sent
	public long lastViewOrderTime = 0;					//Cycle when last change view order was sent
	public long lastTurnOrderTime = 0;					//Cycle when last turn order was sent
	public long lastDashOrderTime = 0;					//Cycle when last dash order was sent
	public long cycleNo;									//Current Cycle
	public double distanceToBall;						//Distance to known ball position
	public double directionToBall;						//Direction towards ball position
	public double distanceToLocation;					//Distance to destination
	public double directionToLocation;					//Direction towards Location
	public double directionToGo;						//Direction that player must go to avoid obstacles
	public double bodyAng;								//Direction of the Body
	public double playerRadius;							//Radius of the player
	public double ballRadius;							//Radius of the Ball
	
	public double distanceToOpp;						//Current distance to Opponent
	public double pSpd;									//Current Player Speed
	public double pDir;									//Current Player Speed Direction
	public double pStam;								//Current Player Stamina
	public double oSpd;									//Current Opponent Speed
	public double oDir;									//Current Opponent Direction
	public double relSpd;								//Current relative Speed
	public double relDir;								//Current relative Direction
	public double relXSpd;								//Current relative X Speed
	public double relYSpd;								//Current relative Y Speed
	public double pXSpd;								//Current Player X Speed
	public double pYSpd;								//Current Player Y Speed
	public double oXSpd;								//Current Opponent X Speed
	public double oYSpd;								//Current Opponent Y Speed
	public double dirToOpp;								//Current direction to Opponent
	public double pPosX;								//current player X Position
	public double pPosY;								//current player Y Position
	public double oPosX;								//current opponent X Position
	public double oPosY;								//current opponent Y Position
	public double execDash;								//executed Dash from last cycle
	public double execTurn;								//executed Turn from last cycle
	public double prevDash;								//previous sent dash
	public double prevSpd;								//previous Speed of the player
	public double nextSpd;								//Speed for next turn
	public Vector2D opp2DSpd;							//Opponent XY Spd
	public MotionPrediction nextLocation = new MotionPrediction();	//Motion prediction object
	public Position playerPosition;						//predicted player Position
	
	public double fuzzyDirection;						//Direction Output of fuzzy Model
	public double fuzzyStrafeAngle;						//Strafe Direction Output of fuzzy Model
	public double fuzzyDash;							//Dash Power Output of fuzzy Model
	public boolean closestIsNull = false;				//Closest Player null flag
	public Player closestPlayer;						//Closest Player
	public boolean init = true;							//initialization flag
	
	private Fuzzy fuzzy;								//Fuzzy Model
	public double[] fuzzyOutputs;						//Output Fuzzy Vector
	
	
	public double inertiaMoment = Self.INERTIA_MOMENT;//Inertia Moment
	
	public PlayerActions(Commands commands, Game game, Self player) {

		this.commands = commands;
		this.game = game;
		this.player = player;
		this.fuzzy = new Fuzzy();
	}
	
	public int getNumber() {
		return this.player.getPlayerNumber();
	}
	
	public void setup(Position position) {
		this.commands.move(position.getX(), position.getY());
	};
	
	public void goTo(Position position, Position ballPosition) {
		
		playerRadius = Server.PLAYER_SIZE;
		ballRadius = Server.BALL_SIZE;
		distanceToBall = this.player.getPosition().getDistanceTo(position) - playerRadius - ballRadius;		//Gets Distance to Ball
		
		if (distanceToBall > Self.KICKABLE_MARGIN){ //Running without the ball
			
		} else { //Running with the ball
			
		}

//		
//		playerRadius = Server.PLAYER_SIZE;
//		ballRadius = Server.BALL_SIZE;
//		distanceToBall = Player.getPosition().getDistanceTo(position) - playerRadius - ballRadius;		//Gets Distance to Ball
//		
//		if (distanceToBall > Player.KICKABLE_MARGIN){ //Running without the ball
//			
//		} else { //Running with the ball
//			
//		}

	}
	
	/**
	 * goTo: method, allows player to aim to a specific position, and if fuzzy is enabled, 
	 * also avoid any encountered obstacle.
	 * @param position position of the player
	 * @param worldModel
	 * @author Luis Carlos González Sua
	 */
	public void goTo(Position position, WorldModel worldModel) {
		cycleNo = this.game.getPlayTime();											//Gets current cycle value
		pStam = Self.STAMINA;														//Gets Current Player Stamina

				
		if (cycleNo <= 1){
			pPosX = this.player.getPosition().getX();
			pPosY = this.player.getPosition().getY();
			pDir = this.player.getPosition().getBodyAngle();
			System.out.println("First Time");
		} else if (cycleNo == worldModel.getGameTimestamp()){
			pPosX = this.player.getPosition().getX();
			pPosY = this.player.getPosition().getY();
			pDir = this.player.getPosition().getBodyAngle();
			System.out.println("On Time");
		} else{
			System.out.println(cycleNo + " Prediction Time");
			try {
				execDash = nextLocation.getDashAt(cycleNo - 2);
				System.out.printf("Dash ejecutado: %.4f %n", execDash);
			} catch(Exception e){
				execDash = 0.0;
				System.out.println("No encontró");
			}
			
			try {
				prevDash = nextLocation.getDashAt(cycleNo - 1);
				System.out.printf("Dash anterior: %.4f %n", prevDash);
			} catch(Exception e){
				prevDash = 0.0;
				System.out.println("No encontró y menos");
			}
			
			try {
				execTurn = nextLocation.getTurnAt(cycleNo - 2);
				System.out.printf("Turn anterior: %.4f %n", execTurn);
			} catch(Exception e){
				execTurn = 0.0;
				System.out.println("No encontró tampoco");
			}
			
				
			if (cycleNo - 1 == worldModel.getGameTimestamp()){
				pPosX = this.player.getPosition().getX();
				pPosY = this.player.getPosition().getY();
				pDir = this.player.getPosition().getBodyAngle();
				System.out.println("Mejor tarde que nunca");
			}
			
			pDir = nextLocation.getNewDirection(pDir, pSpd, execTurn);
			
			playerPosition = nextLocation.getNewPos(pPosX, pPosY, pSpd, pDir, execDash);
			pPosX = playerPosition.getX();
			pPosY = playerPosition.getY();
		
		}
		
		pSpd = Self.AMOUNT_OF_SPEED;												//Gets Current Player Speed
		
		playerPosition = new Position(pPosX, pPosY, pDir, Self.HEAD_ANGLE);

		if ((pPosX >= (position.getX() + positionThreshold)) || 
			(pPosX <= (position.getX() - positionThreshold)) ||
			(pPosY >= (position.getY() + positionThreshold)) ||
			(pPosY <= (position.getY() - positionThreshold))){
			
			distanceToLocation = playerPosition.getDistanceTo(position);			//Gets Distance to Target
			directionToLocation = playerPosition.getAngleFromBodyTo(position);	//Gets Direction to Target
					
			System.out.println(cycleNo + " dis to Tgt: " + distanceToLocation + " Dir to Tgt: " + directionToLocation);
			System.out.println(cycleNo + " X: " + pPosX + " Y: " + pPosY + " pSpd: " + pSpd + " pDir: " + pDir + " pStam: " + pStam);
						
			try {
				closestPlayer = worldModel.getNearestPlayerToPosition(playerPosition);
//				System.out.println("Te Veo Puto!!!");
				closestIsNull =  false;
			}catch (Exception e){
//				System.err.println(cycleNo + " " + e);	
				closestIsNull =  true;
			}
			
			if (!closestIsNull){
				opp2DSpd = closestPlayer.getVelocity();
				oXSpd = opp2DSpd.getXComponent();
				oYSpd = opp2DSpd.getYComponent();
												
				oSpd = opp2DSpd.getMagnitude();
				oDir = opp2DSpd.getDirection();
				
				relDir = oDir - pDir;
				if (relDir > 180){
					relDir = relDir - 360;
				}else if (relDir < -180){
					relDir = relDir + 360;
				}
				
				relXSpd = (oSpd * Math.cos(Math.toRadians(relDir))) + pSpd;
				relYSpd = oSpd * Math.sin(Math.toRadians(relDir));
				
				System.out.println("oSpd: " + oSpd + " oDir: " + oDir +" relDir: " + relDir + " oVX: " + oXSpd + " oVY: " + oYSpd);
				
				distanceToOpp = playerPosition.getDistanceTo(closestPlayer.getCurrentPosition());				
				dirToOpp = playerPosition.getAngleFromBodyTo(closestPlayer.getCurrentPosition());
			} else {
				distanceToOpp = 100;
				relXSpd = 0;
				relYSpd = 0;
				dirToOpp = 0;
			}
						
			
			if(init) {
				init = false; 
				if (playerPosition.getY() < 0){
					this.fuzzy.rightPreference();				
				}else{
					this.fuzzy.leftPreference();				
				}
			} else {
				if (playerPosition.getY() < -chgRulesThreshold){
					this.fuzzy.rightPreference();					
				} else if (playerPosition.getY() > chgRulesThreshold){
					this.fuzzy.leftPreference();
				}
			}
			
			//TODO Activate Deactivate Fuzzy
			
			boolean fuzzyOn = true; //To Enable or Disable Fuzzy Outputs Effect
						
			
			fuzzyOutputs = this.fuzzy.getOutput(distanceToOpp, pStam, relYSpd, relXSpd, dirToOpp);
			
			if (fuzzyOn){
				fuzzyDirection = fuzzyOutputs[1];
				fuzzyStrafeAngle = fuzzyOutputs[2];
				fuzzyDash = fuzzyOutputs[0];
			}else{
				fuzzyDirection = 0; //This value remains to disable fuzzy
				fuzzyStrafeAngle = 0; //This value remains to disable fuzzy
				fuzzyDash = 100; //This value remains to disable fuzzy	
			}
			
			System.out.println("Dash: " + fuzzyDash + " Dir: " + fuzzyDirection + " Strafe: " + fuzzyStrafeAngle);
			
			directionToGo =  directionToLocation + fuzzyDirection; 
						
			if ((directionToGo > directionThreshold) ||
				(directionToGo < -directionThreshold)){				
				if (! turnOrderSent){
					turnOrderSent = true;
					lastTurnOrderTime = cycleNo; //Saves Cycle when order was sent
					turnSent = true;
					
					nextSpd = nextLocation.getNewSpd(pSpd, prevDash);
					
					turnMoment = directionToGo * (1.0 + inertiaMoment * nextSpd);
					
					if (turnMoment > 180){
						turnMoment = 180;
					}else if (turnMoment < -180){
						turnMoment = -180;
					}
					this.commands.turn(turnMoment);
					System.out.println("Turning: " + turnMoment + " direction: " + directionToLocation + " (" 
							+ position.getX() + ", " + position.getY() + ") ");						
				} else {
					if ((lastTurnOrderTime + delayWait) <= cycleNo){
						turnOrderSent = false;
					}					
				}
			}
			
			
			if (distanceToLocation <= nextLocation.distanceToStop(prevDash, pSpd)){
				noDash = true;
			}else {
				noDash = false;
			}
			//TODO Inhabilitar para prueba 3
//			noDash = false;
						
			if (!turnSent) {
				
				if (!noDash){
					this.commands.dash(fuzzyDash, fuzzyStrafeAngle);
					nextLocation.addData(cycleNo, fuzzyDash, 0.0, pSpd, pDir);
					System.out.println("Dashing: " + fuzzyDash);
				} else{
					nextLocation.addData(cycleNo, 0.0, 0.0, pSpd, pDir);
				}
			} else {
				nextLocation.addData(cycleNo, 0.0, turnMoment, pSpd, pDir);
				turnSent = false;
			}
		} else {
			System.out.println(cycleNo + " Ya llegue a: (" + position.getX() + "," + position.getY() + "), " +
					"Estoy en: (" + playerPosition.getX() + "," + playerPosition.getY() + ")");
			System.out.println("pSpd: " + Self.AMOUNT_OF_SPEED);
		}
	}
	
	public void pass(Position position) {
		
	}
	
	public void catchBall() {
		
	}
	
	public void searchBall() {
		playMode = this.game.getPlayMode();						//Gets current PlayMode
		neckAng = Self.HEAD_ANGLE;						//Gets current neck angle
		cycleNo = (int)this.game.getGlobalTime();				//Gets current cycle value
		
		if(playMode.equals("play_on")) {

			if(!Self.VIEW_MODE_WIDTH.equals("normal")){

				if(changeViewSent){
					if((lastViewOrderTime + delayWait) <= cycleNo) {
						changeViewSent = false;
					}
				}else {
					this.commands.changeView("normal");
					lastViewOrderTime = cycleNo;
					changeViewSent = true;
				}
			}
			
			if(neckOrderSent) {
				if((lastNeckOrderTime + delayWait + addDelay) <= cycleNo) {
					neckOrderSent = false;
				}
			}else {
				neckOrderSent = true;
				lastNeckOrderTime = cycleNo;
				if((Math.abs(neckAng) > 23.0) && (Math.abs(neckAng) < 27.0)) {
					neckSearchMoment = -(neckAng / Math.abs(neckAng)) * 50.0;
				}else{
					neckSearchMoment = 25.0 - neckAng;
				}
				this.commands.turnNeck((int)neckSearchMoment);
			}
			
		}else {
			if(!(Self.VIEW_MODE_WIDTH.equals("wide"))){
				if(changeViewSent){
					if((lastViewOrderTime + delayWait + addDelay) <= cycleNo) {
						changeViewSent = false;
					}
				}else {
					this.commands.changeView("wide");
					lastViewOrderTime = cycleNo;
					changeViewSent = true;
				}
			}
			
			if(neckOrderSent) {
				if((lastNeckOrderTime + delayWait) <= cycleNo) {
					neckOrderSent = false;
				}
			}else {
				neckOrderSent = true;
				lastNeckOrderTime = cycleNo;
				if(Math.abs(neckAng) >= 88) {
					neckSearchMoment = -(neckAng / Math.abs(neckAng)) * 180.0;
				}else {
					if(this.player.getPosition().getY() >= 0.0){
						neckSearchMoment = -90.0;
					}else {
						neckSearchMoment = 90.0;
					}
				}
				this.commands.turnNeck((int)neckSearchMoment);
			}
			
		}
	}
	
	public void explore(Position position) {
		distanceToBall = this.player.getPosition().getDistanceTo(position);				//Gets Distance to Ball
		directionToBall = this.player.getPosition().getAngleFromBodyTo(position);		//Gets Direction to Ball
		neckAng = Self.HEAD_ANGLE;												//Gets current neck angle
		cycleNo = (int)this.game.getGlobalTime();										//Gets current cycle value
		
		if (Math.abs(directionToBall) > 22.5){
			if(!(Self.VIEW_MODE_WIDTH.equals("wide"))){
				if(changeViewSent){
					if((lastViewOrderTime + delayWait) <= cycleNo) {
						changeViewSent = false;
					}
				}else {
					this.commands.changeView("wide");
					lastViewOrderTime = cycleNo;
					changeViewSent = true;
				}
			}
			
			if(neckOrderSent) {
				if((lastNeckOrderTime + delayWait + addDelay) <= cycleNo) {
					neckOrderSent = false;
				}
			}else {
				neckOrderSent = true;
				lastNeckOrderTime = cycleNo;
				if((Math.abs(neckAng) > 66) && (Math.abs(neckAng) < 70)) {
					neckSearchMoment = -(neckAng / Math.abs(neckAng)) * 135.0;
				}else{
					neckSearchMoment = 68 - neckAng;
				}
				this.commands.turnNeck((int)neckSearchMoment);
			}
			
		}else {
			if (distanceToBall > 25) {
							
				if(neckOrderSent) {
					if((lastNeckOrderTime + delayWait + addDelay) <= cycleNo) {
						neckOrderSent = false;
					}
				}else {
					neckOrderSent = true;
					lastNeckOrderTime = cycleNo;
					if((Math.abs(neckAng) > 21) && (Math.abs(neckAng) < 25)) {
						neckSearchMoment = -(neckAng / Math.abs(neckAng)) * 45.0;
					}else{
						neckSearchMoment = 23 - neckAng;
					}
					this.commands.turnNeck((int)neckSearchMoment);
				}
				
				if(!(Self.VIEW_MODE_WIDTH.equals("normal"))){
					if(changeViewSent){
						if((lastViewOrderTime + delayWait) <= cycleNo) {
							changeViewSent = false;
						}
					}else {
						this.commands.changeView("normal");
						lastViewOrderTime = cycleNo;
						changeViewSent = true;
					}
				}
				
			}else {
							
				if(neckOrderSent) {
					if((lastNeckOrderTime + delayWait + addDelay) <= cycleNo) {
						neckOrderSent = false;
					}
				}else {
					neckOrderSent = true;
					lastNeckOrderTime = cycleNo;
					if(Math.abs(neckAng) > 2) {
						neckSearchMoment = -neckAng;
					}else{
						neckSearchMoment = 0;
					}
					this.commands.turnNeck((int)neckSearchMoment);
				}
				
				if(!(Self.VIEW_MODE_WIDTH.equals("narrow"))){
					if(changeViewSent){
						if((lastViewOrderTime + delayWait) <= cycleNo) {
							changeViewSent = false;
						}
					}else {
						this.commands.changeView("narrow");
						lastViewOrderTime = cycleNo;
						changeViewSent = true;
					}
				}
			}
		}
		
	}
	public void lookTo(Position position) {
		distanceToBall = this.player.getPosition().getDistanceTo(position);				//Gets Distance to Ball
		directionToBall = this.player.getPosition().getAngleFromBodyTo(position);		//Gets Direction to Ball
		neckAng = Self.HEAD_ANGLE;												//Gets current neck angle
		cycleNo = (int)this.game.getGlobalTime();										//Gets current cycle value

		if(Math.abs(directionToBall) >= 110){ //Ignores top angle
			if(!(Self.VIEW_MODE_WIDTH.equals("wide"))){
				if(changeViewSent){
					if((lastViewOrderTime + delayWait) <= cycleNo) {
						changeViewSent = false;
					}
				}else {
					this.commands.changeView("wide");
					lastViewOrderTime = cycleNo;
					changeViewSent = true;
				}
			}
			
			if(exploreOrderSent) {  //Aims towards ball
				if((lastExploreOrderTime + delayWait) <= cycleNo) {
					exploreOrderSent = false;
				}
			}else {
				exploreOrderSent = true;
				lastExploreOrderTime = cycleNo;
				if(((directionToBall > 0) && (neckAng <= 88)) || ((directionToBall < 0) && (neckAng >= -88))) {
					exploreMoment = directionToBall - neckAng;
					
					if(exploreMoment > 180.0){
						exploreMoment = 180.0;
					}else if (exploreMoment < -180.0) {
						exploreMoment = -180.0;
					}
					this.commands.turnNeck((int)exploreMoment);
				}
			}
	
		}else {  //Aims to ball
			if(exploreOrderSent) {  //Aims towards ball
				if((lastExploreOrderTime + delayWait) <= cycleNo) {
					exploreOrderSent = false;
				}
			}else {
				exploreOrderSent = true;
				lastExploreOrderTime = cycleNo;
				if((Math.abs(directionToBall - neckAng) >= 2) && (Math.abs(directionToBall) < 110)){
					exploreMoment = directionToBall - neckAng;
					
					if(exploreMoment > 180.0){
						exploreMoment = 180.0;
					}else if (exploreMoment < -180.0) {
						exploreMoment = -180.0;
					}
					this.commands.turnNeck((int)exploreMoment);
				}
			}
			
			if (distanceToBall >= 20){
				if(!(Self.VIEW_MODE_WIDTH.equals("normal"))){
					if(changeViewSent){
						if((lastViewOrderTime + delayWait) <= cycleNo) {
							changeViewSent = false;
						}
					}else {
						this.commands.changeView("normal");
						lastViewOrderTime = cycleNo;
						changeViewSent = true;
					}
				}
			}else {
				if(!(Self.VIEW_MODE_WIDTH.equals("narrow"))){
					if(changeViewSent){
						if((lastViewOrderTime + delayWait) <= cycleNo) {
							changeViewSent = false;
						}
					}else {
						this.commands.changeView("narrow");
						lastViewOrderTime = cycleNo;
						changeViewSent = true;
					}
				}
			}
			
		}
		
	}
	
	public Commands perform() {
		return this.commands;
	}
	
	public Position getOpponentPosition(WorldModel worldModel) {
		
		double xoppPosition = this.player.getPosition().getX();
//		double yoppPosition = this.player.getPosition().getY();
		
		//TODO Para Test 3 se usa esto

			Position oppPosition = new Position(40.0, 0.0, 0.0);
				
		try {
			closestPlayer = worldModel.getNearestPlayerToPosition(this.player.getPosition());
			closestIsNull =  false;
		}catch (Exception e){
			closestIsNull =  true;
		}
		
		if (!closestIsNull){
			oppPosition = closestPlayer.getCurrentPosition();
		} else {
			if (xoppPosition > 0){
				oppPosition = new Position(-40.0, 0.0);
			}else {
				oppPosition = new Position(40.0, 0.0);
			}
		}
		
		return oppPosition;
	}
	
	public Position getPlayerPosition(){
		return this.player.getPosition();
	}
	
}
